# ros2_app_server.py
import asyncio
import websockets
import json
import threading
import rclpy
from aiohttp import web
import pathlib
import subprocess
from rclpy.action import ActionClient
from nav_msgs.msg import OccupancyGrid, Path
from geometry_msgs.msg import Twist, PoseStamped, PoseWithCovarianceStamped
from nav2_msgs.action import NavigateToPose
from tf_transformations import quaternion_from_euler, euler_from_quaternion
import hashlib
import time
from tf2_msgs.msg import TFMessage


class ROS2Communicator:
    """ROS2通信管理器，处理地图订阅、速度控制和导航目标"""

    def __init__(self, server):
        # 保存服务器引用，用于向前端发送状态
        self.server = server

        # 地图数据管理
        self.map_data = None
        self.last_map_hash = None
        self.new_map_available = False
        self.last_map_update_time = 0

        # 机器人位置和路径数据
        self.robot_pose = None
        self.nav_path = None
        self.new_pose_available = False
        self.new_path_available = False
        self.last_pose_update_time = 0
        self.position_source = None  # 'amcl' 或 'odom'
        self.nav_goal_handle = None  # 保存当前导航目标句柄

        # 速度指令管理
        self.vel_cmd_queue = asyncio.Queue()
        self.is_running = True

        # 初始化ROS2节点
        rclpy.init(args=None)
        self.node = rclpy.create_node('web_control_node')

        # 地图订阅者
        self.map_subscriber = self.node.create_subscription(
            OccupancyGrid,
            '/map',
            self._on_map_received,
            10
        )

        # 机器人位置订阅者 - AMCL
        self.amcl_subscriber = self.node.create_subscription(
            PoseWithCovarianceStamped,
            '/amcl_pose',
            lambda msg: self._on_pose_received(msg, 'amcl'),
            10
        )

        # 订阅TF消息以获取odom到base_link/base_footprint的变换
        self.tf_subscriber = self.node.create_subscription(
            TFMessage,
            '/tf',
            self._on_tf_received,
            10
        )

        # 导航路径订阅者
        self.path_subscriber = self.node.create_subscription(
            Path,
            '/plan',
            self._on_path_received,
            10
        )

        # 速度指令发布者
        self.vel_publisher = self.node.create_publisher(
            Twist,
            '/cmd_vel',
            10
        )

        # 导航Action客户端
        self.nav_client = ActionClient(self.node, NavigateToPose, 'navigate_to_pose')

        # 启动ROS2节点自旋线程
        self.ros_spin_thread = threading.Thread(target=self._spin_ros_node, daemon=True)
        self.ros_spin_thread.start()

        # 启动速度指令发布协程
        self.loop = asyncio.new_event_loop()
        asyncio.set_event_loop(self.loop)
        self.loop.create_task(self._publish_vel_commands())
        self.loop_thread = threading.Thread(target=self.loop.run_forever, daemon=True)
        self.loop_thread.start()

        # 定期检查数据更新状态
        self.watchdog_thread = threading.Thread(target=self._data_watchdog, daemon=True)
        self.watchdog_thread.start()

        self.node.get_logger().info("[ROS2Communicator] 初始化完成")

    # 发送命令执行状态到前端
    async def _send_command_status(self, status, message):
        """通过WebSocket向前端发送命令执行状态"""
        status_msg = {
            "type": "command_status",
            "status": status,
            "message": message
        }
        msg_str = json.dumps(status_msg)

        # 遍历所有连接的客户端发送状态
        for client in list(self.server.connected_clients):
            try:
                await client.send(msg_str)
                self.node.get_logger().info(f"[状态发送] {status}: {message}")
            except websockets.exceptions.ConnectionClosed:
                self.node.get_logger().warn("[状态发送失败] 客户端已断开")

    # 处理多步骤命令执行
    def execute_commands(self, command_type, steps):
        """在单独线程中执行命令序列，避免阻塞主循环"""
        threading.Thread(
            target=self._command_execution_thread,
            args=(command_type, steps),
            daemon=True
        ).start()

    # 命令执行线程
    def _command_execution_thread(self, command_type, steps):
        """实际执行命令的线程函数"""
        self.node.get_logger().info(f"[命令执行] 开始 {command_type} 序列，共 {len(steps)} 步")

        # 逐个执行步骤
        for i, step in enumerate(steps):
            step_num = i + 1
            try:
                # 发送步骤开始状态
                asyncio.run_coroutine_threadsafe(
                    self._send_command_status(
                        "running",
                        f"执行步骤 {step_num}/{len(steps)}: {step}"
                    ),
                    self.loop
                ).result()

                # 执行命令（使用shell=True允许管道和环境变量）
                result = subprocess.run(
                    step,
                    shell=True,
                    check=True,  # 若命令返回非0状态码则抛出异常
                    capture_output=True,
                    text=True,
                    timeout=30  # 每个命令超时时间30秒
                )

                # 命令执行成功
                asyncio.run_coroutine_threadsafe(
                    self._send_command_status(
                        "success",
                        f"步骤 {step_num} 成功: {step}"
                    ),
                    self.loop
                ).result()
                self.node.get_logger().info(f"[命令成功] 步骤 {step_num}: {step}")

            except subprocess.TimeoutExpired:
                # 命令超时
                asyncio.run_coroutine_threadsafe(
                    self._send_command_status(
                        "error",
                        f"步骤 {step_num} 超时: {step}"
                    ),
                    self.loop
                ).result()
                self.node.get_logger().error(f"[命令超时] 步骤 {step_num}: {step}")
                break  # 超时后停止后续步骤

            except subprocess.CalledProcessError as e:
                # 命令执行失败
                error_msg = f"返回码 {e.returncode}: {e.stderr[:100]}"  # 限制错误信息长度
                asyncio.run_coroutine_threadsafe(
                    self._send_command_status(
                        "error",
                        f"步骤 {step_num} 失败: {error_msg}"
                    ),
                    self.loop
                ).result()
                self.node.get_logger().error(f"[命令失败] 步骤 {step_num}: {error_msg}")
                break  # 失败后停止后续步骤

            except Exception as e:
                # 其他异常
                asyncio.run_coroutine_threadsafe(
                    self._send_command_status(
                        "error",
                        f"步骤 {step_num} 异常: {str(e)}"
                    ),
                    self.loop
                ).result()
                self.node.get_logger().error(f"[命令异常] 步骤 {step_num}: {str(e)}")
                break

            # 步骤间短暂延迟，确保环境变量生效（如source命令）
            time.sleep(1)

        # 所有步骤执行完成
        if i + 1 == len(steps):  # 所有步骤都执行完成
            asyncio.run_coroutine_threadsafe(
                self._send_command_status(
                    "success",
                    f"{command_type} 命令序列全部执行完成"
                ),
                self.loop
            ).result()
            self.node.get_logger().info(f"[命令完成] {command_type} 所有步骤执行完毕")

    def _on_map_received(self, msg: OccupancyGrid):
        """处理接收到的地图消息"""
        try:
            # 改进哈希计算，结合元数据和实际数据
            metadata_str = f"{msg.info.width},{msg.info.height},{msg.info.resolution},{msg.info.origin.position.x},{msg.info.origin.position.y},{msg.info.map_load_time.sec},{msg.info.map_load_time.nanosec}"
            data_str = '|'.join(map(str, msg.data))  # 使用|分隔符避免数字拼接歧义
            combined_str = f"{metadata_str}|{data_str}"
            current_hash = hashlib.md5(combined_str.encode()).hexdigest()

            # 更新最后更新时间
            self.last_map_update_time = time.time()

            if current_hash != self.last_map_hash or not self.map_data:
                self.last_map_hash = current_hash
                self.map_data = {
                    'width': msg.info.width,
                    'height': msg.info.height,
                    'resolution': msg.info.resolution,
                    'origin_x': msg.info.origin.position.x,
                    'origin_y': msg.info.origin.position.y,
                    'data': msg.data
                }
                self.new_map_available = True
                self.node.get_logger().info(
                    f"[地图更新] 栅格尺寸: {msg.info.width}×{msg.info.height}, "
                    f"数据长度: {len(msg.data)}"
                )
        except Exception as e:
            self.node.get_logger().error(f"[地图处理错误] {str(e)}")

    def _on_pose_received(self, msg: PoseWithCovarianceStamped, source: str):
        """处理接收到的机器人定位消息"""
        try:
            pose = msg.pose.pose
            # 将四元数转换为欧拉角以获取偏航角
            _, _, yaw = euler_from_quaternion([
                pose.orientation.x,
                pose.orientation.y,
                pose.orientation.z,
                pose.orientation.w
            ])

            self.robot_pose = {
                'x': pose.position.x,
                'y': pose.position.y,
                'theta': yaw,
                'source': source
            }
            self.position_source = source
            self.new_pose_available = True
            self.last_pose_update_time = time.time()  # 更新时间戳
            self.node.get_logger().info(
                f"[位置更新] {source}: x={pose.position.x:.2f}, y={pose.position.y:.2f}, θ={yaw:.2f}")
        except Exception as e:
            self.node.get_logger().error(f"[位置处理错误] {str(e)}")

    def _on_tf_received(self, msg: TFMessage):
        """从TF消息中提取odom到base_link/base_footprint的变换作为备选位置源"""
        # 只有当AMCL没有数据时才使用odom数据
        if self.position_source == 'amcl':
            return

        try:
            for transform in msg.transforms:
                # 支持odom→base_link和odom→base_footprint两种变换
                if transform.header.frame_id == 'odom' and transform.child_frame_id in ['base_link', 'base_footprint']:
                    self.node.get_logger().info(
                        f"[TF更新] 检测到 {transform.header.frame_id}→{transform.child_frame_id} 变换"
                    )

                    # 转换TF消息到PoseWithCovarianceStamped格式并处理
                    pose_msg = PoseWithCovarianceStamped()
                    pose_msg.header = transform.header
                    pose_msg.pose.pose.position.x = transform.transform.translation.x
                    pose_msg.pose.pose.position.y = transform.transform.translation.y
                    pose_msg.pose.pose.position.z = transform.transform.translation.z
                    pose_msg.pose.pose.orientation = transform.transform.rotation

                    self._on_pose_received(pose_msg, 'odom')  # 标记为odom源
                    break
        except Exception as e:
            self.node.get_logger().error(f"[TF处理错误] {str(e)}")

    def _on_path_received(self, msg: Path):
        """处理接收到的导航路径消息"""
        try:
            path_points = []
            for pose_stamped in msg.poses:
                path_points.append({
                    'x': pose_stamped.pose.position.x,
                    'y': pose_stamped.pose.position.y
                })

            # 始终更新路径数据
            self.nav_path = path_points
            self.new_path_available = True
            self.node.get_logger().info(f"[路径更新] 路径点数量: {len(path_points)}")
        except Exception as e:
            self.node.get_logger().error(f"[路径处理错误] {str(e)}")

    async def _publish_vel_commands(self):
        """定期发布速度控制指令"""
        while self.is_running:
            try:
                linear_x, angular_z = await asyncio.wait_for(
                    self.vel_cmd_queue.get(),
                    timeout=1.0
                )

                twist = Twist()
                twist.linear.x = linear_x
                twist.angular.z = angular_z
                self.vel_publisher.publish(twist)
                self.vel_cmd_queue.task_done()

                await asyncio.sleep(0.05)

            except asyncio.TimeoutError:
                continue
            except Exception as e:
                self.node.get_logger().error(f"[发布错误] {str(e)}")
                await asyncio.sleep(0.1)

    def send_velocity_command(self, linear_x: float, angular_z: float):
        """发送速度控制指令到队列"""
        linear_x = max(-1.0, min(1.0, linear_x))
        angular_z = max(-2.0, min(2.0, angular_z))

        try:
            asyncio.run_coroutine_threadsafe(
                self.vel_cmd_queue.put((linear_x, angular_z)),
                self.loop
            ).result()
        except Exception as e:
            self.node.get_logger().error(f"[指令错误] {str(e)}")

    def send_navigation_goal(self, x: float, y: float, theta: float = 0.0):
        """发送导航目标点 - 修复版本，兼容不同ROS2版本"""
        if not self.nav_client.wait_for_server(timeout_sec=5.0):
            self.node.get_logger().error("[导航错误] Nav2服务器未启动")
            return

        # 取消当前可能的导航 - 兼容不同ROS2版本
        if self.nav_goal_handle:
            try:
                # 尝试新版本方法
                self.nav_goal_handle.cancel_goal_async()
            except AttributeError:
                try:
                    # 尝试旧版本方法
                    self.nav_client.cancel_goal(self.nav_goal_handle)
                except Exception as e:
                    self.node.get_logger().warn(f"[导航取消警告] 无法取消当前目标: {str(e)}")
            self.nav_goal_handle = None

        # 发送导航目标前清除现有路径
        self.nav_path = None
        self.new_path_available = True

        q = quaternion_from_euler(0, 0, theta)
        pose = PoseStamped()
        pose.header.frame_id = 'map'
        pose.header.stamp = self.node.get_clock().now().to_msg()
        pose.pose.position.x = x
        pose.pose.position.y = y
        pose.pose.orientation.x = q[0]
        pose.pose.orientation.y = q[1]
        pose.pose.orientation.z = q[2]
        pose.pose.orientation.w = q[3]

        goal_msg = NavigateToPose.Goal()
        goal_msg.pose = pose

        future = self.nav_client.send_goal_async(goal_msg)
        future.add_done_callback(self._on_goal_response)

    def cancel_navigation(self):
        """取消当前导航任务，并立即停止机器人"""
        canceled = False
        if self.nav_goal_handle:
            try:
                # 尝试新版本方法
                self.nav_goal_handle.cancel_goal_async()
            except AttributeError:
                try:
                    # 尝试旧版本方法
                    self.nav_client.cancel_goal(self.nav_goal_handle)
                except Exception as e:
                    self.node.get_logger().warn(f"[导航取消警告] 无法取消当前目标: {str(e)}")
            self.nav_goal_handle = None
            canceled = True
            self.node.get_logger().info("[导航状态] 导航任务已取消")

        # 无论是否有导航任务，都清除路径并发送停止指令
        self.nav_path = None
        self.new_path_available = True

        # 发布零速度指令立即停止机器人
        zero_twist = Twist()
        self.vel_publisher.publish(zero_twist)
        self.node.get_logger().info("[紧急停止] 发布零速度指令")

        return canceled

    def _on_goal_response(self, future):
        """处理导航目标响应 - 增加异常处理"""
        try:
            goal_handle = future.result()
            if not goal_handle.accepted:
                self.node.get_logger().warn("[导航状态] 目标被拒绝")
                return

            self.nav_goal_handle = goal_handle  # 保存目标句柄以便后续取消
            self.node.get_logger().info("[导航状态] 目标已接受")
            result_future = goal_handle.get_result_async()
            result_future.add_done_callback(self._on_goal_complete)
        except Exception as e:
            self.node.get_logger().error(f"[导航响应错误] {str(e)}")
            self.nav_goal_handle = None

    def _on_goal_complete(self, future):
        """修复导航完成回调错误，增加异常处理"""
        try:
            # 正确获取导航状态：status来自ActionResult，而非Result对象
            action_result = future.result()
            status = action_result.status
            self.node.get_logger().info(f"[导航状态] 导航完成，状态码: {status}")

            # 导航状态码说明：
            # 1: 已接受, 2: 执行中, 3: 已取消, 4: 已成功, 5: 已失败
            if status == 4:
                self.node.get_logger().info("[导航状态] 导航成功完成")
            elif status == 5:
                self.node.get_logger().warn("[导航状态] 导航失败")

        except AttributeError as e:
            # 兼容不同版本的ROS2消息结构
            self.node.get_logger().info(f"[导航状态] 导航完成 (版本兼容模式)")
        except Exception as e:
            self.node.get_logger().error(f"[导航完成回调错误] {str(e)}")
        finally:
            self.nav_goal_handle = None  # 清除目标句柄
            # 导航完成后清除路径
            self.nav_path = None
            self.new_path_available = True

    def _spin_ros_node(self):
        """ROS节点自旋线程 - 增加异常捕获，防止线程崩溃"""
        while rclpy.ok() and self.is_running:
            try:
                rclpy.spin_once(self.node, timeout_sec=0.1)
            except Exception as e:
                self.node.get_logger().error(f"[自旋线程错误] {str(e)}，继续运行...")
                # 短暂休眠后继续，避免错误时CPU占用过高
                time.sleep(0.1)

    def _data_watchdog(self):
        """数据监控线程，检测数据是否停止更新"""
        while self.is_running:
            current_time = time.time()

            # 检测地图是否超过30秒未更新
            if current_time - self.last_map_update_time > 30 and self.map_data:
                self.node.get_logger().warn("[数据监控] 地图超过30秒未更新，强制标记为新数据")
                self.new_map_available = True
                self.last_map_update_time = current_time  # 重置计时器

            # 检测位置是否超过5秒未更新
            if current_time - self.last_pose_update_time > 5 and self.robot_pose:
                self.node.get_logger().warn("[数据监控] 位置超过5秒未更新，强制标记为新数据")
                self.new_pose_available = True
                self.last_pose_update_time = current_time  # 重置计时器

            time.sleep(1)  # 每秒检查一次

    def shutdown(self):
        self.is_running = False
        self.loop.stop()
        rclpy.shutdown()
        self.node.get_logger().info("[ROS2Communicator] 已关闭")


class RobotWebServer:
    """机器人Web服务器，提供WebSocket和HTTP服务"""

    def __init__(self,
                 host: str = '0.0.0.0',
                 ws_port: int = 8765,
                 http_port: int = 8000):
        self.host = host
        self.ws_port = ws_port
        self.http_port = http_port
        self.connected_clients = set()
        # 创建ROS通信器时传入服务器引用
        self.ros_communicator = ROS2Communicator(self)
        self.broadcast_interval = 0.2  # 广播频率5Hz

        print(f"[服务器] WebSocket服务: ws://{host}:{ws_port}")
        print(f"[服务器] HTTP控制界面: http://{host}:{http_port}")

    async def _broadcast_robot_data(self):
        """向所有客户端广播机器人数据（强化版，逐个发送避免阻塞）"""
        if not self.connected_clients:
            self.ros_communicator.node.get_logger().info("[广播提示] 无在线客户端，跳过广播")
            return

        messages = []

        # 地图数据
        if self.ros_communicator.new_map_available and self.ros_communicator.map_data:
            map_data = self.ros_communicator.map_data
            self.ros_communicator.new_map_available = False

            grid_data_str = ','.join(map(str, map_data['data']))

            map_msg = {
                "type": "grid_map",
                "width": map_data['width'],
                "height": map_data['height'],
                "resolution": map_data['resolution'],
                "origin_x": map_data['origin_x'],
                "origin_y": map_data['origin_y'],
                "data_str": grid_data_str
            }
            messages.append(map_msg)
            self.ros_communicator.node.get_logger().info(
                f"[广播准备] 待发送地图数据: 尺寸{map_data['width']}×{map_data['height']}"
            )

        # 机器人位置
        if self.ros_communicator.new_pose_available and self.ros_communicator.robot_pose:
            pose = self.ros_communicator.robot_pose
            self.ros_communicator.new_pose_available = False

            pose_msg = {
                "type": "robot_pose",
                "x": pose['x'],
                "y": pose['y'],
                "theta": pose['theta'],
                "source": pose['source']
            }
            messages.append(pose_msg)
            self.ros_communicator.node.get_logger().info(
                f"[广播准备] 待发送位置数据: {json.dumps(pose_msg)}"
            )

        # 导航路径
        if self.ros_communicator.nav_path is not None:
            path = self.ros_communicator.nav_path
            path_msg = {
                "type": "nav_path",
                "path": path
            }
            messages.append(path_msg)
            self.ros_communicator.node.get_logger().info(
                f"[广播准备] 待发送路径数据: {len(path)}个点"
            )

        # 逐个客户端发送（避免单个客户端阻塞影响全部）
        for message in messages:
            msg_str = json.dumps(message)
            # 使用列表副本迭代，避免迭代中修改集合导致错误
            for client in list(self.connected_clients):
                try:
                    await client.send(msg_str)
                    self.ros_communicator.node.get_logger().info(
                        f"[广播成功] 向客户端发送: {message['type']}"
                    )
                except websockets.exceptions.ConnectionClosed:
                    self.ros_communicator.node.get_logger().warn(
                        "[广播警告] 客户端已断开，移除连接"
                    )
                    self.connected_clients.remove(client)
                except Exception as e:
                    self.ros_communicator.node.get_logger().error(
                        f"[广播失败] 发送{message['type']}时出错: {str(e)}"
                    )

    async def _handle_websocket_client(self, websocket):
        """处理WebSocket客户端连接（增加详细日志）"""
        self.connected_clients.add(websocket)
        client_addr = websocket.remote_address  # 获取客户端IP和端口
        self.ros_communicator.node.get_logger().info(
            f"[客户端连接] 新客户端: {client_addr}，在线数: {len(self.connected_clients)}"
        )

        try:
            async for message in websocket:
                try:
                    data = json.loads(message)
                    self.ros_communicator.node.get_logger().info(
                        f"[收到客户端消息] {client_addr}: {data.get('type')}"
                    )

                    if data.get("type") == "control":
                        linear_x = float(data.get("linear_x", 0.0))
                        angular_z = float(data.get("angular_z", 0.0))
                        self.ros_communicator.send_velocity_command(linear_x, angular_z)

                    elif data.get("type") == "request_data":
                        # 强制刷新所有数据
                        self.ros_communicator.new_map_available = True
                        self.ros_communicator.new_pose_available = True
                        self.ros_communicator.new_path_available = True
                        await self._broadcast_robot_data()

                    elif data.get("type") == "request_update":
                        # 只发送现有数据的更新，不强制刷新
                        await self._broadcast_robot_data()

                    elif data.get("type") == "set_goal":
                        x = float(data.get("x", 0.0))
                        y = float(data.get("y", 0.0))
                        self.ros_communicator.send_navigation_goal(x, y)
                        self.ros_communicator.node.get_logger().info(
                            f"[收到新导航目标] x={x:.2f}, y={y:.2f}"
                        )

                    # 新增：处理取消导航指令
                    elif data.get("type") == "cancel_navigation":
                        canceled = self.ros_communicator.cancel_navigation()
                        if canceled:
                            await self.ros_communicator._send_command_status(
                                "success", "导航任务已取消"
                            )
                        else:
                            await self.ros_communicator._send_command_status(
                                "warning", "当前没有导航任务可取消"
                            )

                    # 处理多步骤命令
                    elif data.get("type") == "launch_commands":
                        command_type = data.get("command")
                        steps = data.get("steps", [])
                        if not steps:
                            self.ros_communicator.node.get_logger().warn("[命令错误] 步骤为空")
                            continue
                        # 交给ROS通信器执行命令
                        self.ros_communicator.execute_commands(command_type, steps)

                except json.JSONDecodeError:
                    self.ros_communicator.node.get_logger().warn(
                        f"[消息错误] {client_addr}发送无效JSON: {message[:50]}..."
                    )
                except Exception as e:
                    self.ros_communicator.node.get_logger().error(
                        f"[消息处理错误] {str(e)}"
                    )

        except websockets.exceptions.ConnectionClosed:
            self.ros_communicator.node.get_logger().info(
                f"[客户端断开] {client_addr} 连接关闭"
            )
        except Exception as e:
            self.ros_communicator.node.get_logger().error(
                f"[客户端错误] {client_addr}: {str(e)}"
            )
        finally:
            if websocket in self.connected_clients:
                self.connected_clients.remove(websocket)
            self.ros_communicator.node.get_logger().info(
                f"[客户端断开] 剩余在线: {len(self.connected_clients)}"
            )

    async def _handle_http_request(self, request: web.Request):
        """处理HTTP请求，提供静态文件访问"""
        path = request.path
        if path == '/':
            filename = 'robot_control_interface.html'
        else:
            filename = path[1:]

        server_dir = pathlib.Path(__file__).resolve().parent
        file_path = server_dir / filename

        if not file_path.exists() or not file_path.is_file():
            return web.Response(status=404, text="文件不存在")

        content_type = 'text/html'
        if filename.endswith('.css'):
            content_type = 'text/css'
        elif filename.endswith('.js'):
            content_type = 'application/javascript'
        elif filename.endswith(('.jpg', '.jpeg')):
            content_type = 'image/jpeg'
        elif filename.endswith('.png'):
            content_type = 'image/png'

        try:
            with open(file_path, 'rb') as f:
                content = f.read()
            return web.Response(body=content, content_type=content_type)
        except Exception as e:
            self.ros_communicator.node.get_logger().error(f"[文件读取错误] {str(e)}")
            return web.Response(status=500, text="服务器内部错误")

    async def start_services(self):
        """同时启动WebSocket和HTTP服务"""
        ws_server = await websockets.serve(
            self._handle_websocket_client,
            self.host,
            self.ws_port,
            ping_interval=30,
            ping_timeout=10
        )

        http_app = web.Application()
        http_app.router.add_get('/{tail:.*}', self._handle_http_request)

        runner = web.AppRunner(http_app)
        await runner.setup()
        http_site = web.TCPSite(runner, self.host, self.http_port)
        await http_site.start()

        self.ros_communicator.node.get_logger().info("[服务器] 所有服务启动完成")

        await asyncio.gather(
            self._data_broadcast_loop(),
            ws_server.wait_closed()
        )

    async def _data_broadcast_loop(self):
        """高频广播机器人数据"""
        while True:
            await self._broadcast_robot_data()
            await asyncio.sleep(self.broadcast_interval)

    def run(self):
        """启动服务器主循环"""
        try:
            asyncio.run(self.start_services())
        except KeyboardInterrupt:
            self.ros_communicator.shutdown()
            print("\n[服务器] 已手动停止")


if __name__ == "__main__":
    server = RobotWebServer()
    server.run()